`timescale 1ns / 1ps
//////////////////////////////////////////////////////////////////////////////////
// Company: 
// Engineer: 
// 
// Create Date: 2024/08/09 13:30:05
// Design Name: 
// Module Name: clark_park
// Project Name: 
// Target Devices: 
// Tool Versions: 
// Description: 
// 
// Dependencies: 
// 
// Revision:
// Revision 0.01 - File Created
// Additional Comments:
// 
//////////////////////////////////////////////////////////////////////////////////


module clark_park(
    input                   sys_clk_i           ,
    input                   sys_rst_n_i         ,
    
    input                   clark_park_en_i     ,
    output                  clark_park_ack_o    ,

    input signed[17:0]      cordic_sin_i        ,   
    input signed[17:0]      cordic_cos_i        ,
    input signed[31:0]      Ia_i                ,
    input signed[31:0]      Ib_i                ,
    input signed[31:0]      Ic_i                ,

    output reg signed[31:0] Ialpha_o            ,
    output reg signed[31:0] Ibeta_o             ,
    output reg signed[31:0] Iq_o                ,
    output reg signed[31:0] Id_o            
);
localparam SQRT3        =       $signed(32'd37937);   // (1 / sqrt(3))^16 = d37937
localparam SQRT3_mul_2  =       $signed(32'd75674);   //（(1 / sqrt(3))*2)^16 = d75674



reg[5:0]    cnt;
reg         dealing;


reg signed[63:0]   temp1;
reg signed[63:0]   temp2;
reg signed[64:0]   temp12_sum;

reg signed[63:0]   a;
reg signed[63:0]   b;
reg signed[63:0]   c;
reg signed[63:0]   d;

reg signed[31:0]   aa;
reg signed[31:0]   bb;
reg signed[31:0]   cc;
reg signed[31:0]   dd;

assign clark_park_ack_o = ( cnt == 'd13 ) ? 1'b1 : 1'b0;

always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 )
        dealing <= 1'b0;
    else if( clark_park_ack_o == 1'b1 )
        dealing <= 1'b0;
    else if( clark_park_en_i == 1'b1 )
        dealing <= 1'b1;
    else
        dealing <= 1'b0;
end

always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 )
        cnt <= 'd0;
    else if( dealing == 1'b1 )
        cnt <= cnt + 1'b1;
    else
        cnt <= 'd0;
end

always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 ) begin
        temp1 <= 'd0;
        temp2 <= 'd0;      
    end
    else if( cnt == 'd1 ) begin
        temp1   <= SQRT3 * Ib_i;
        temp2   <= SQRT3_mul_2 * Ic_i;    
    end
    else begin
        temp1 <= temp1;
        temp2 <= temp2;
    end
end
always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 )
        temp12_sum <= 'd0;
    else if( cnt ==  'd3)
        temp12_sum <= temp1 + temp2;
    else
        temp12_sum <= temp12_sum;
end



always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 ) begin
        Ialpha_o    <= 'd0;
        Ibeta_o     <= 'd0;      
    end
    else if( cnt == 'd5 ) begin
        Ialpha_o    <= Ib_i;
        Ibeta_o     <= temp12_sum >>> 16;    
    end
    else begin
        Ialpha_o <= Ialpha_o;
        Ibeta_o  <= Ibeta_o;
    end
end



always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 ) begin
        a <= 'd0;
        b <= 'd0;   
        c <= 'd0;
        d <= 'd0;      
    end
    else if( cnt == 'd7 ) begin
        a <= Ialpha_o * cordic_sin_i;
        b <= Ibeta_o * cordic_cos_i;   
        c <= Ialpha_o * cordic_cos_i;
        d <= Ibeta_o * cordic_sin_i;
    end
    else begin
        a <= a;
        b <= b;
        c <= c;
        d <= d;
    end
end

always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 ) begin
        aa <= 'd0;
        bb <= 'd0;   
        cc <= 'd0;
        dd <= 'd0;      
    end
    else if( cnt == 'd9) begin
        aa <= a >>> 16;
        bb <= b >>> 16;
        cc <= c >>> 16;
        dd <= d >>> 16;
    end
    else ;
end


always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 ) begin
        Iq_o <= 'd0;
        Id_o <= 'd0;      
    end
    else if( cnt == 'd11 ) begin
        Iq_o <= bb - aa;
        Id_o <= cc + dd;     
    end
    else begin
        Iq_o <= Iq_o;
        Id_o <= Id_o;
    end
end









// localparam  S_IDLE        =   'd0;
// localparam  S_STEP_0      =   'd1;
// localparam  S_STEP_1      =   'd2;
// localparam  S_STEP_2      =   'd3;
// localparam  S_STEP_3      =   'd4;
// localparam  S_STEP_4      =   'd5;
// localparam  S_STEP_5      =   'd6;
// localparam  S_STEP_6      =   'd7;
// localparam  S_STEP_7      =   'd8;
// localparam  S_STEP_8      =   'd9;
// localparam  S_ACK         =   'd10;


// reg                 a_mul_en;
// reg signed[31:0]    a_mul_a;
// reg signed[31:0]    a_mul_b;
// wire signed[63:0]   a_mul_out;

// reg                 b_mul_en;
// reg signed[31:0]    b_mul_a;
// reg signed[31:0]    b_mul_b;
// wire signed[63:0]   b_mul_out;

// reg                 addsub_en;
// reg                 addsub_sel;
// reg signed[63:0]    addsub_a;
// reg signed[63:0]    addsub_b;
// reg signed[64:0]   addsub_out;


// reg[4:0] state , next_state;
// reg[3:0] cnt;

// assign clark_park_ack_o = ( state == S_ACK ) ? 1'b1 : 1'b0;

// always@( posedge sys_clk_i ) begin
//     if( state == S_IDLE )
//         cnt <= 'd0;
//     else if( state != next_state )
//         cnt <= 'd0;
//     else
//         cnt <= cnt + 1'b1;
// end



// always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
//     if( sys_rst_n_i == 1'b0 )
//         state <= S_IDLE;
//     else
//         state <= next_state;
// end


// always@(*) begin
//     case (state)
//         S_IDLE: 
//             if( clark_park_en_i == 1'b1 )
//                 next_state <= S_STEP_0;
//             else
//                 next_state <= S_IDLE;
//         S_STEP_0:
//             if( cnt == 'd4 )
//                 next_state <= S_STEP_1;
//             else
//                 next_state <= S_STEP_0;
//         S_STEP_1:
//             if( cnt == 'd8 )
//                 next_state <= S_STEP_2;
//             else
//                 next_state <= S_STEP_1;
//         S_STEP_2:
//             next_state <= S_STEP_3;
//         S_STEP_3:
//             if( cnt == 'd4 )
//                 next_state <= S_STEP_4;
//             else
//                 next_state <= S_STEP_3;
//         S_STEP_4:
//             if( cnt == 'd8 )
//                 next_state <= S_STEP_5;
//             else
//                 next_state <= S_STEP_4;
//         S_STEP_5:
//             next_state <= S_STEP_6;
//         S_STEP_6:
//             if( cnt == 'd4 )
//                 next_state <= S_STEP_7;
//             else
//                 next_state <= S_STEP_6;
//         S_STEP_7:
//             if( cnt == 'd8 )
//                 next_state <= S_STEP_8;
//             else
//                 next_state <= S_STEP_7;
//         S_STEP_8:
//             next_state <= S_ACK;
//         S_ACK:
//             next_state <= S_IDLE;
//         default:  next_state <= S_IDLE;
//     endcase
// end


// always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
//     if( sys_rst_n_i == 1'b0 ) begin
//         a_mul_en <= 1'b0;
//         a_mul_a  <= 'd0;
//         a_mul_b  <= 'd0;
//         b_mul_en <= 1'b0;
//         b_mul_a  <= 'd0;
//         b_mul_b  <= 'd0 ;
//     end
//     else if( state == S_STEP_0 ) begin
//         a_mul_en <= 1'b1;
//         a_mul_a <= SQRT3;
//         a_mul_b <= Ia_i;
//         b_mul_en <= 1'b1;
//         b_mul_a <= SQRT3_mul_2;
//         b_mul_b <= Ib_i;
//     end
//     else if(state == S_STEP_3) begin
//         a_mul_en <= 1'b1;
//         a_mul_a <= Ialpha_o;
//         a_mul_b <= cordic_sin_i;
//         b_mul_en <= 1'b1;
//         b_mul_a <= Ibeta_o;
//         b_mul_b <= cordic_cos_i;
//     end
//     else if( state == S_STEP_6 ) begin
//         a_mul_en <= 1'b1;
//         a_mul_a <= Ialpha_o;
//         a_mul_b <= cordic_cos_i;
//         b_mul_en <= 1'b1;
//         b_mul_a <= Ibeta_o;
//         b_mul_b <= cordic_sin_i;
//     end 
//     else begin
//         a_mul_en <= 1'b0;
//         a_mul_a <= a_mul_a;
//         a_mul_b <= a_mul_b;
//         b_mul_en <= 1'b0;
//         b_mul_a <= b_mul_a;
//         b_mul_b <= b_mul_b;
//     end
// end

// always@( posedge sys_clk_i or negedge sys_rst_n_i  ) begin
//     if( sys_rst_n_i == 1'b0 ) begin
//         addsub_en   <= 1'b0;
//         addsub_sel  <= 1'b0;
//         addsub_a    <=  'd0;
//         addsub_b    <=   'd0;
//     end
//     else if( state == S_STEP_1 ) begin
//         addsub_en   <= 1'b1;
//         addsub_sel  <= 1'b0;
//         addsub_a    <= a_mul_out;
//         addsub_b    <=  b_mul_out;
//     end
//     else if( state == S_STEP_4 ) begin
//         addsub_en   <= 1'b1;
//         addsub_sel  <= 1'b1;
//         addsub_a    <= b_mul_out;
//         addsub_b    =  a_mul_out;
//     end
//     else if( state == S_STEP_7 ) begin
//         addsub_en   <= 1'b1;
//         addsub_sel  <= 1'b0;
//         addsub_a    <= a_mul_out;
//         addsub_b    =  b_mul_out;
//     end
//     else begin
//         addsub_en   <= 1'b0;
//         addsub_sel  <= 1'b0;
//         addsub_a    <= addsub_a;
//         addsub_b    =  addsub_b;
//     end
// end

// always@(  posedge sys_clk_i or negedge sys_rst_n_i ) begin
//     if( sys_rst_n_i == 1'b0 ) begin
//         Ialpha_o <= 'd0;
//         Ibeta_o  <= 'd0;
//     end
//     else if( state == S_STEP_2 ) begin
//         Ialpha_o <= Ia_i;
//         Ibeta_o  <= ( addsub_out >>> 32);
//     end
//     else begin
//         Ialpha_o <= Ialpha_o;
//         Ibeta_o  <= Ibeta_o;
//     end
// end

// always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
//     if( sys_rst_n_i == 1'b0 )
//         Iq_o <= 'd0;
//     else if( state == S_STEP_5 )
//         Iq_o <= ( addsub_out >>> 32);
//     else
//         Iq_o <= Iq_o;
// end

// always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
//     if( sys_rst_n_i == 1'b0 )
//         Id_o <= 'd0;
//     else if( state == S_STEP_8 )
//         Id_o <= ( addsub_out >>> 32);
//     else
//         Id_o <= Id_o;
// end


// assign a_mul_out = a_mul_a * a_mul_b;
// // mult_gen_32_32 mult_gen_32_32_a_hp
// // (
// //     .CLK        (       sys_clk_i       ),  
// //     .A          (       a_mul_a         ),
// //     .B          (       a_mul_b         ),  
// //     .CE         (       a_mul_en        ),
// //     .P          (       a_mul_out       )
// // );
// assign b_mul_out = b_mul_a * b_mul_b;
// // mult_gen_32_32 mult_gen_32_32_b_hp
// // (
// //     .CLK        (       sys_clk_i       ),  
// //     .A          (       b_mul_a         ),
// //     .B          (       b_mul_b         ),  
// //     .CE         (       b_mul_en        ),
// //     .P          (       b_mul_out       )
// // );

// always@( posedge sys_clk_i ) begin
//     if( addsub_sel ==  1'b1 )
//         addsub_out <= addsub_a + addsub_b;
//     else
//         addsub_out <= addsub_a - addsub_b;
// end
// // c_addsub_64 c_addsub_64_hp
// // (
// //     .A          (       addsub_a        ),
// //     .B          (       addsub_b        ),
// //     .CLK        (       sys_clk_i       ),
// //     .ADD        (       addsub_sel      ),
// //     .CE         (       addsub_en       ),
// //     .S          (       addsub_out      )
// // );

endmodule
